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Abstract 

In this paper, we first review the theory of symmetry-preserving observers and we 
mention some recent results. Then, we apply the theory to Extended Kalman Filter- 
based Simultaneous Localization and Mapping (EKF SLAM). It allows to derive a new 
(symmetry-preserving) Extended Kalman Filter for the non-linear SLAM problem that 
possesses convergence properties. We also prove a special choice of the gains ensures 
global exponential convergence. 

1 Introduction 

Symmetries and Lie groups have been widely used for feedback control in robotics, see 
e.g. [7] [T3]. More generally control of systems possessing symmetries has also been 
studied for quite a long time, see e.g. [5] E] ■ The use of symmetries and Lie groups for 
observer design is more recent [HE]. The main properties of those observers are based on 
the reduction of the estimation error complexity. When the symmetry group coincides 
with the state space (observers on Lie groups), the error equation can be particularly 
simple 4 . This property has been used to derive non-linear observers with (almost) 
global convergence properties for several localisation problems [111 5] [TS]. Recently 
[5] established a link between observer design and control of systems on Lie group by 
proving a non-linear separation principle on Lie groups. 

This paper proposes to recap the main elements of the theory along with some recent 
results, and to apply it to the domain of Extended Kalman Filter-based Simultaneous 
Localization and Mapping (EKF SLAM). It is organized as follows: Section 2 is a brief 
recap on linear observers. In Section 3 we recap the theory of symmetry-preserving 
observers [3] and mention some recent results [5] [6]. In Section 4 we apply it in a 
straightforward way to EKF SLAM. In Section 5 some results for the special case of 
observers for invariant systems on Lie groups [3] are recalled. In Section 6, it is proved 
that those results can be (surprisingly) applied to EKF SLAM. We derive a simple 
globally convergent observer for the non-linear problem. We also propose a modified 
EKF such that the covariance matrix and the gain matrix behave as if the system was 
linear and time-invariant. Such non-linear convergence guarantees for EKF SLAM are 
new to the author's knowledge. The author would like to mention and to thank his 
regular co-authors on the subject of symmetry-preserving observers : Philippe Martin, 
Pierre Rouchon, and Erwan Salaiin. 
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2 Luenberger observers, extended Kalman filter 



2.1 Observers for linear systems 

Observers are meant to compute an estimation of the state of a dynamical system from 
several sensor measurements. Let x £ R n denote the state of the system, u £ R m be 
the inputs (a set of m known scalar variables such as controls, constant parameters, 
etc.). We assume the sensors provide measurements y € W that can be expressed as a 
function of the state and the inputs. When the underlying dynamical model is a linear 
differential equation, and the output is a linear function as well, the system can be 
written 

— x = Ax + Bu, y = Cx + Du. (1) 
A Luenberger observer (or Kalman filter) writes 

—x = Ax + Bu — L ■ (Cx + Du — y), (2) 

where x is the estimated state, and L is a gain matrix that can be freely chosen. We 
see that the observer consists in a copy of the system dynamics Ax + Bu, plus a cor- 
rection term L(Cx + Du — y) that "corrects" the trusted dynamics in function of the 
discrepancy between the estimated output y — Cx + Du and the measured output y. 

One important issue is the choice (or "tuning") of the gain matrix L. The Lu- 
enberger observer is based on a choice of a fixed matrix L. In the Kalman filter 
two positive definite matrices M and N denote the covariance matrices of the state 
noise and measurement noise, and L relies on a Ricatti equation : L — PC T N, where 
±P = AP + PA T + A/ -1 - PC T NCP. As M and N must be defined by the user, they 
can be viewed as tuning matrices. 

In both cases the observer has the form (J2j with L constant or not. Let x — x — x be 
the estimation error, and let us compute the differential equation satisfied by the error. 
We have 

^-x = (A + LC)x. (3) 
dt 

As the goal of the observer is to find an estimate of x, we want x to go to zero. When the 
system is observable, one can always find L such that x asymptotically exponentially 
goes to zero, and the negative real part of the eigenvalues of A + LC can be freely 
assigned. We see that the theory is particularly simple as the error equation ((3| is 
autonomous, i.e. it does not depend on the trajectory followed by the system. In 
particular, the input term u has vanished in ([3]). The well-known separation principle 
stems from this fact. 



2.2 Some popular extensions to nonlinear systems 

Consider a general nonlinear system 

^-x = f(x,u), y = h(x,u), (4) 

where x € X C R" is the state, u G U C R m the input, and y e y C W the output. 
Mimicking the linear case, a class of popular nonlinear observers writes 

—x = f(x, u) - L(x, y, t) ■ (h(x, u) - y(t)) , (5) 

where the gain matrix can depend on the variables x, y, t. The error equation can still be 
computed, but as the system is nonlinear, it does not necessarily lead to an appropriate 
gain matrix L. Indeed we have ^x — f(x, u(t)) — f(x, u(t)) — L(x, y(t),t) ■ (h(x, u(t)) — 
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y(t)). The error equation is no longer autonomous, and the problem of finding L such 
that x goes asymptotically to zero can not be solved in the general case. 

The most popular observer for nonlinear systems is the Extended Kalman Filter 
(EKF). The principle is to linearize the system around the estimated trajectory, build 
a Kalman filter for the linear model, and implement it on the nonlinear system. The 
EKF has the form ([5|, where the gain matrix is computed the following way: 

df,„ \ „ dh . 

A=-£(x,u) (6) 

L = PC T N' 1 ^-P — AP + PA T + M - PC T N~ 1 CP. (7) 

The EKF has two main flaws when compared to the KF for time- invariant linear systems. 
First the linearized system around any trajectory is generally time-varying and the 
covariance matrix does not tend to a fixed value. Then, when x — x is large the linearized 
error equation can be a very erroneous approximation of the true error equation. 



3 Symmetry-preserving observers 

3.1 Symmetry group of a system of differential equations 

Let G be a group, and M be a set. A group action can be defined on M if to any g £ G 
on can associate a diffeomorphic transformation cj> g : M —¥ M such that (j>gh = <t>g ° 4>h, 
and {<j>g)~ 1 = <t>g-~i-> *- e -> the group multiplication corresponds to the transformation 
composition, and the reciprocal elements correspond to reciprocal transformations. 

Definition 1 G is a symmetry group of a system of differential equations defined on 
M if it maps solutions to solutions. In this case we say the system is invariant. 

Definition 2 A vector field w on M is said invariant if the system jj^z = w(z) is 
invariant. 

Definition 3 A scalar invariant is a function I : M — > R such that I((j) g (z)) — I(z) for 
all g e G. 

Salar invariants and invariant vector fields can be built via Cartan's moving frame 
method [14] , 

Definition 4 A moving frame is a function 7 : M — > G such that y(4>g( z )) — 9 • l{ z ) 
for all g, z. 

Suppose dim G = r < dim M. Under some mild assumptions on the action (free, 
regular) there exists locally a moving frame. The sets O z = {(j> g (z), g £ G} are called 
the group orbits. Let K be a cross-section to the orbits. A moving frame can be built 
locally via implicit functions theorem as the solution g = 7(2) of the equation <j> g (z) = k 
where k £ O z n K. A complete set of functionnaly independent invariants is given by 
the non-constant components of <^ 7 ( z j(z). Figure 1 illustrates those definitions and the 
moving frame method. 

3.2 Symmetry group of an observer 

Consider the general system ((4]). Consider also the local group of transformations on 
X x U defined for any x, u, g by 

cj>g(x,U) = (ip g (x),1pg(u)), (8) 

where ip g and xp g correspond to separate local group of transformations of X and U. 

Proposition 1 The system j^x = f(x, it) is said invariant if it is invariant to the group 
action ([8]). 
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group orbits 



Figure 1: An illustrative example. M = R 2 , and the symmetry group is made of horizontal 
translations. We have cj> g (zi,Z2) = (zi + g,Z2) T where g £ G = R. In local rectifying coordinates, 
every invariant system can be represented by a similar figure (under mild assumptions on the group 
action). Left: Invariant system. The symmetry group maps each integral line of the vector field into 
another integral line. Right: Moving frame method. K is a cross-section to the orbits and 7(2:1,22) 
is the group element that maps [zi, 22) to K along the orbit. For exemple if K is the set {21 = 0}, 
the moving frame is 7(21, 22) = z\ and a complete set of invariants is I(zi, 22) = 22. 

The group maps solutions to solutions if we have -^X — f(X,U), where (X, U) = 
(tpg(x),ipg(u)) for all g £ G. We understand from this definition, that u can denote 
the control variables as usual, but it also denotes every feature of the environment that 
makes the system not behave the same way after it has been transformed (via ip g ). The 
action of ip g is meant to allow some features of the environment to be also moved over. 
We would like the observer to be an invariant system for the same symmetry group. 

Definition 5 The observer (f5j is invariant or "symmetry-preserving" if it is an invari- 
ant system for the group action (x,x,u,y) [(p g (x),ipg(x),ipg(u),h((p g (x),'il>g(u))) . 
In this case, the structure of the observer mimicks the nonlinear structure of the system. 
Let us recall how to build such observers (see [3] for more details). To do so, we need 
the output to be equivariant: 

Definition 6 The output is equivariant if there exists a group action on the output 
space (via p g ) such that h((p g (x), ip g (u)) = p g (h(x, u)) for all g, x, u. 

We will systematically assume the output is equivariant. Let us define an invariant 
output error, instead of the usual linear output error y — y: 

Definition 7 The smooth map (x, u, y) E(x, u, y) £ R p is an invariant output error 
if 

• Eup g (x),tl) g (u), p g (y)) = E(x,u,y) for all x,u,y (invariant) 

• the map y h-> E(x, u, y) is invertible for all x, u (output) 

• E(x,u,h(x,u)) =0forallx,u (error) 

An invariant error is given (locally) by E(x,u,y) = p 1 (x,u){y) — Pj(x,u)(y)- Finally, an 
invariant frame (wi, ...,w n ) on X, which is a set of n linearly point-wise independent 
invariant vector fields, i.e (wi(x), ...,w n (x)) is a basis of the tangent space to X at x. 
Once again such a frame can be built (locally) via the moving frame method. 

Proposition 2 {5C The system 4:X = F(x, u, y) is an invariant observer for the invari- 
ant system -^x — f(x, u) if and only if: 

n 

F(x,u,y) = f(x,u) + ^2Ci(l(x,u), E(x,u,y))wi(x) (9) 
i=i 
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Figure 2: Vehicle taking relative measurements to environmental landmarks. 



where E is an invariant output error, I(x, u) is a complete set of scalar invariants, the 
Li's are smooth functions such that for all x, d (/ (i,it),0) = 0, and (w%, w n ) is an 
invariant frame. 

The gains d must be tuned in order to get some convergence properties if possible, 
and their magnitude should depend on the trade-off between measurement noise and 
convergence speed. The convergence analysis of the observer often relies on an invariant 
state-error: 

Definition 8 The smooth map (x, x) h-» r/(x, x) £ R" is an invariant state error if 
r)((p g (x),tp g (x)) = r](x,x) (invariant), the map x t— > r/(x,x) is invertible for all x (state), 
and r/(x,x) = (error). 

3.3 An example: symmetry-preserving observers for posi- 
tive linear systems 

The linear system j^x = Ax, y — Cx admits scalings G = R* as a symmetry group via 
the group action 4> g (x) = gx. Every linear observer is obviously an invariant observer. 
The unit sphere is a cross-section K to the orbits. A moving frame maps the orbits to 
the sphere and thus writes 7(2;) = l/ll^ll £ G. A complete set of invariants is given 
locally by n — 1 independent coordinates of <j)^i x \(x) = a;/||x||. Let I(x) € R n_1 be a 
complete set of independent invariants. I(x) and ||z|| provide alternative coordinates 
named base and fiber coordinates. Moreover the system has a nice triangular structure 
in those coordinates. One can prove that -^I(x(t)) is an invariant function and thus it 
is necessarily of the form g(I). As a result we have 4;I(x) = g(I(x)) which does not 
depend on ||x||. 

We have thus the following (general) result : if the restriction of the vector field on 
the cross-section is a contraction, it suffices to define a reduced observer on the orbits i.e. 
in our case a norm observer (which means that a scalar output suffices for observability). 
This is the case for instance when A is a matrix whose coefficients are stricly positive 
(according to the Perron- Froebenius theorem). This fact was recently used in [6] to 
derive invariant asymptotic positive observers for positive linear systems. 



4 A first application to EKF SLAM 

Simultaneous localisation and mapping (SLAM) addresses the problem of building a 
map of an environment from a sequence of sensor measurements obtained from a moving 
robot. A solution to the SLAM problem has been seen for more than twenty years as a 
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"holy grail" in the robotics community since it would be a means to make a robot truly 
autonomous in an unknown environment. A very well-known approach that appeared in 
the early 2000's is the EKF SLAM [8]. Its main advantage is to formulate the problem in 
the form of a state-space model with additive Gaussian noise and to provide convergence 
properties in the linear case (i.e. straight line motion). Indeed, the key idea is to include 
the position of the several landmarks (i.e. the map) in the state space. This solution 
has been gradually replaced by other techniques such as FastSLAM, Graph SLAM etc. 

In the framework of EKF SLAM, the problem of estimating online the trajectory 
of the robot as well as the location of all landmarks without the need for any a priori 
knowledge of location can be formulated as follows [Sj. The vehicle state is defined 
by the position in the reference frame (earth-fixed frame) x £ R 2 of the centre of the 
rear axle and the orientation of the vehicle axis 8. The vehicle trusted motion relies on 
non-holonomic constraints. The landmarks are modeled as points and represented by 
their position in the reference frame p; £ R 2 where 1 < i < JV. u, u € R are control 
inputs. Both vehicle and landmark states are registered in the same frame of reference. 
In a determistic setting (state noises turned off), the time evolution of the (huge) state 
vector is 

x = uReei, 8 = uv, p\ = 1 < i < N (10) 

where ei = (1, 0) T and Rg is the rotation matrix of angle 8. Supposing that the data 
association between landmarks from one instant to the next is correctly done, the ob- 
servation model for the i-th landmark (disregarding measurement noise) is its position 
seen from the vehicle's frame: Zi = R-e(pi — x). The standard EKF SLAM estimator 
has the form 

d d - N d N 

—x = uRgei + L x (z k -z k ), —8 = uv + ^2,L h g (z k -z k ), —pi = ) L i (z k - z k ), l<i<N 

i i 

(11) 

where Zi = R_§(pi — x) and where the Li's are the lines of L tuned via the EKF equations 

©-IB- 
Here the group of symmetry of the system corresponds to Galilean invariances, and it 

is made of rotations and translations of the plane SE(2). Indeed, looking at Figure 2, it is 
obvious that the equations of motion are the same whether the first horizontal axis of the 
reference frame is pointing North, or East, or in any direction. For g = (xo, 6>o) £ SE(2), 
the action of the group on the state space is <fi g (x, 8,pi) — (Re g x + xo,8 + 8o, Re Pi + xo) 
and ip g (u,v) = u,v. The output is also unchanged by the group transformation as it 
is expressed in the vehicle frame and is thus insensitive to rotations and translations of 
the reference frame. Applying the theory of the last section, the observer above can be 
"invariantized" , yielding the following invariant observer: 

d N d - N d N 

— x = uR § e 1 +R § (^L k x {z k ~z k )), —8 = uv + ^Lg(z k - z k ), —pi = Rs(Y] Lj{z k - z k )) 

\l2) 

It is easy to see that the invariant observer is much more meaningful, especially if the L\s 
are chosen as constant matrices [3]- Indeed, one could really wonder if it is sensible to 
correct vectors expressed in the reference frame directly with measurements expressed 
in the vehicle frame. To be convinced, consider the following simple case : suppose 
8 — 8 = x — x — remain fixed. We have 4;(pi — p») = Li(pi —pi). Choosing Li = — k I 
yields ^\\pi — Pi || 2 = — fc||j3j — Pi\\ 2 leading to a correct estimation of landmark pi. Now 
suppose that the vehicle has changed its orientation and 8 = 8 — n/2. The output error 
is now R- n /2(Pi — Pi)- With an observer of the form the same choice Li = — k I 
yields -§^\\pi ~Pi\\ =0 and the landmark is not correctly estimated. On the other hand, 
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with l|12|> we have in both cases 4f\\pi — Pi|| 2 = — k\\pi — Pi|| 2 ensuring convergence of p 
towards p. 

Constant gains is a special (simple) choice, but the observer gains can also be tuned 
via Kalman equations. Indeed on can define noises on the linearized invariant error 
system and tune the Li's via Kalman equations (see Invariant EKF method [2 ). To 
sum up, any Luenberger observer or EKF can be invariantized via equations (112ft . This 
yields in the author's opinion a much more meaningful non-linear observer that is well- 
adapted to the problem's structure. The invariantized observer (|12jl is simply a version 
of (|ll|l which is less sensitive to change of coordinates, and even if no proof can support 
this claim we believe it can only improve the performances of (JTTJ . 

5 Particular case where the state space coincides 
with its symmetry group 

Over the last half decade, invariant observers on Lie groups for low-cost aided inertial 
navigation have been studied by several teams in the world, [111 [3J [TS] to name a 
few. Several powerful convergence results have been obtained. They are all linked to 
the special properties of the invariant state error on a Lie group. To recap briefly the 
construction of invariant observers on Lie groups [3] , we assume that the symmetry group 
G is a matrix group, and that X — G. The system is assumed to be invariant to left 
multiplications i.e. — XQ(t). We have indeed for any g £ G that -^(gX) — (gX)Q,. 
For instance the motion of the vehicle in the considered SLAM problem x — uRgei, 9 = 
uv can be viewed as a left-invariant system on the Lie group SE(2) via the matrix 
representation 

x = ( n Re i) , n = ( " x ue S] , with Uw = ( -™) 

V0lx2 l J ' V°1X2 J ' \UV J 

Suppose the output y = h(X) is equivariant, i.e. there exists a group action on the 
output space such that h(gX) — p g (X). In this case the invariant observer ([9]) can be 
written intrinsically 

with L(e) — where e is the group identity element. The invariant state error is the 
natural group difference r\ — X~^X and the error equation is 

— r)= [n,rj\ + rjL o hirj" 1 ) 

A remarkable fact is that the error equation only depends on rj and fi, whereas the 
system is non-linear and the error should also depend on X (think about the EKF 
which is based on a linearization around any X at each time). Moreover, if Q = est, the 
error equation is clearly autonomous. Thus the motion primitives generated by constant 
Q are special trajectories called "permanent trajectories" . Around such trajectories one 
can always achieve local convergence (as soon as the linearized system is observable). 

It is worth noting this property was recently used to derive a non-linear separation 
principle on Lie groups [5]. It applies to some cart-like underactuated vehicles and some 
underwater or aerial fully actuated vehicles. 

An even more interesting case occurs when the output satisfies right-equivariance 
i.e., h(Xg) = p g (h(X)). In this case we let the input be u — £1 and we consider the 
action of G by right multiplication, i.e. tp g (X) = Xg and V , s(^) = g~ The output is 
equivariant as h(tp g (X)) = p g o h(X). The invariant observer associated with this group 
of symmetry writes -^X — XQ, + L(pj^^ 1 (y))X. The invariant state error is rj = XX~ Y 
and the error equation is 

—j] = XtlX' 1 + L(/i(77 _1 ))?7 - XQ.X- 1 = L(/i(?7" 1 ))?7 (13) 
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The error equation is completely autonomous ! In particular the linearized system 
around any trajectory is the same time-invariant system. Autonomy is the key for 
numerous powerful convergence results for observers on Lie groups see e.g. [101 1151 B]. 

6 A new result in EKF SLAM 

In this section we propose a new non-linear observer for EKF SLAM with guaranteed 
convergence properties. In the SLAM problem the state space is much bigger than its 
symmetry group. The orbits have dimension 3 and thus there are iV + 1 — 3 + 2 = 
N invariants (dimension of the cross-section, see Fig.l). Thus an autonomous error 
equation seems to be out of reach. Suprinsingly considering the symmetry group of 
rotations and translations in the vehicle frame yields such a result. A simple trick 
makes it obvious. Consider the following matrix representation: 

\Uix2 ij \Uix2 i j \Uix2 i) ) \Uix2 uy 

The equations of the system ([TO]) can be written j-X = XQ., —Pj = P;f^, 1 < i < N 
and the system can be viewed as a left-invariant dynamics system on the (huge) Lie 
group G x • ■ • x G. Let r\ x = XX~ 1 ,rji = PiPf 1 be the invariant state error. The 
system has the invariant output errors Y = Rg(zi — Zi), i.e. (Yi l) T = — rj x )H 
for 1 < 2 < N where H = (0i X 2 l) T - Consider the following invariant observer 
f t X = Xft + L x (Yi, ■ ■ ■ ,Y N )X, f t P = Pitli + Liffi,--- ,Y N )P. From (ED, the 
(non-linear) error equation is completely autonomous reminding the linear case ©. It 
implies the following global convergence result for the non-linear deterministic system: 

Proposition 3 Consider the SLAM problem (|10[l without noise. The following observer 

^-§ = uv, —x=uR§ei, —pi = kiRg(zi — Zi) 

with ki > is such that 4j{R^{z\ — zi)) = —ki R§(zi — Zi), i.e., all the estimation errors 
(2i — Zi), 1 < i < N converge globally exponentially to zero with rate ki, which means the 
vehicle trajectory and the map are correctly identified. The parameter ki must be tuned 
according to the level of noise associated to landmark i, and vehicle sensors ' noise. 

If one wants to define noise covariance matrices M, N to tune the observer (and compute 
an estimation P of the covariance error matrix at each time), it is also possible to define 
a modified EKF with guaranteed convergence properties: 

Proposition 4 Consider the SLAM problem (|10[) . Let E = (Rg(zi — Zj))i<i<jv be the 
invariant output error. Let ez be the vertical axis. Consider the observer 

—§ = uv + C e (E), —S; = uR s ei+Ce(E)e3Ax + C x (E), ^p % = £ e (E)e 3 A p + &(E) 

Let 7] = (9,x,p~i, ■ ■ ■ ,p~n) be the invariant state error where 8 = 8 — 8, x = x — Rg-x, fii = 
Pi — RgPi. The state error equation is autonomous, i.e. ^r) only depends on r/. It is thus 
completely independent of the trajectory and of u(t),v(t). The linearized error equation 
writes 4:5rj = {LC)5 r q where L can be freely chosen and C is a fixed matrix. As in the 
usual EKF method, one can define covariance matries M , N , build a Kalman filter for 
the linearized system, i.e. tune L via the usual equations Q i.e. P = M — PC 7 N~ 1 CP , 
L = PC T N -1 , and implement it on the non-linear model. All the convergence results 
on P and L valid for stationnary systems ([T]) with A = 0, B = 0, D = apply. 

Simulations (Fig. [3| with one landmark and noisy measurements indicate the mod- 
ified EKF (IEKF) behaves very similarly, or slightly better than the EKF, but the gain 
matrix tends quickly to a fixed matrix L independently from the trajectory and the 
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inputs u, v. So the Invariant EKF proposed in this paper 1- is incomparably cheaper 
computationaly as it relies on a constant matrix L that can be computed offline once 
and for all (the number of landmarks can thus be much increased) 2- is such that the 
linearized error system is stable as soon as LC has negative eigenvalues, which is easy 
to verify. 

Remark 1 The calculations above are valid on SE(3) and the results apply to 6 DOF 
SLAM. 
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Figure 3: Simulations with one landmark and a car moving over a circular path with a 20% noise. 
Up: 1-Estimated vehicle trajectory (plain blue line) and landmark position (dashed green line) with 
Invariant EKF, 2-Estimation with the usual EKF, 3- true vehicle trajectory (plain blue line) and 
landmark position (green cross). After a short transient, the trajectory is correctly identified for 
both observers (up to a rotation-translation). Bottom : 1-coefScients of L(t) over time for Invariant 
EKF, 2-coefncients of L(t) for EKF. Wee see the EKF gain matrix is permanently adapting to the 
motion of the car (right) whereas its invariant counterpart (left) is directly expressed in well-adapted 
variables. 
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